package robot.calibeta;

import robot.calibeta.Cartographer.Cape;
import chair.Behavior;
import chair.Function;
import chair.core.Diary;

/**
 * Le robot se tourne vers le cap indiqué par Cartographer puis va droit devant
 * lui en suivant la ligne sur laquelle il se trouve. Dès qu'il sort de la piste
 * (voit du blanc) il effectue une correction de trajectoire destinée à le
 * remettre sur les rails.
 * 
 * Aura besoin qu'on lui indique où se trouve Organ destiné à connaître couleur
 * afin de suivre une ligne noire au sol.
 */
public class BehaviorGoStraight extends Behavior {
	/** Quelle est la couleur présente sous le robot ? */
	private OrganEyeColor _seeColor;
	/**
	 * Utilisé pour savoir si encore but à atteindre ou bien s'il faut faire le
	 * mort
	 */
	private Cartographer _magellan;
	/** A franchit récemment la ligne */
	private boolean _hasCrossedLine = false;
	/** De combien de degrés le robot tournera quand il zigzaguera */
	protected float _turnAngle = 0;
	/**
	 * De combien de degrés tourne le robot va tourner pour zigzaguer,
	 * ititialisé à 0 : va tout droit tant que n'a pas rencontré sa niche
	 */
	protected float _currentTurnAngle = 0;

	public BehaviorGoStraight(Function fct, OrganEyeColor seeColor,
			Cartographer magellan) {
		this(fct, seeColor, magellan, Motion.TURN_ANGLE);
	}

	/**
	 * Cette version du constructeur permet de définir explicitement l'angle
	 * avec lequel le robot doit zigzaguer (sinon donné par Motion.TURN_ANGLE).
	 * Utile pour justement désactiver le zig-zag.
	 * 
	 * @param fct
	 *            Fonction rattachée au comportement
	 * @param seeColor
	 *            Organe suivant le tracé
	 * @param magellan
	 *            Cartographer, gère la représentation spatiale
	 * @param turnAngle
	 *            angle utilsé par le robot pour zigzaguer
	 */
	public BehaviorGoStraight(Function fct, OrganEyeColor seeColor,
			Cartographer magellan, float turnAngle) {
		super(fct, "GoStraight", false);
		_seeColor = seeColor;
		_magellan = magellan;
		_turnAngle = turnAngle;
		setSpontaneous(true);
	}

	/**
	 * Par convention on place le robot dans l'axe. Ira tout droit jusqu'à
	 * rencontrer carrefour. Pour les autres fois, on va lui imprimer un premier
	 * parcours une fois tourné vers son cap. En effet, que ce soit les tous
	 * premiers pas du robot, qu'on vienne de rencontrer un carrefour ou bien
	 * qu'on ait juste de définit un itinéraire, on part dans la bonne
	 * direction.
	 */
	protected void init() {
		_hasCrossedLine = true;

		// Pas encore vu la niche : on avance
		if (!_magellan.hasDiscoveredKennel()) {
			Diary.logln("En avant toute jusqu'à niche ! ");
			Motion.forward();
			return;
		} else {
			// commence à louvoyer
			_currentTurnAngle = _turnAngle;
		}

		// Le robot change de cap si besoin
		Cape nextCape = _magellan.getNextCape();
		if (nextCape != null) {
			Diary.logln("Cap sélectionné : " + nextCape);
			Motion.changeCapeAngle(nextCape.getAngle());
			// Pour commencer on dévie légèrement vers l'Ouest
			Motion.rotateFromCape(_currentTurnAngle);
			Motion.forward();
		} else {
			Diary.logln(toString() + " Attention, aucun cap sélectionné.");
			Motion.stop();
			// On permet à un autre comportement de prendre la main
			setStoppable(true);
		}
	}

	/**
	 * Le robot vient de franchir la ligne, on redresse son cap
	 */
	private void turn() {
		// On cherche à aller verd le "Nord".
		if (Motion.getCapeDeviation() > 0)
			Motion.rotateFromCape(_currentTurnAngle);
		else
			Motion.rotateFromCape(-_currentTurnAngle);
		Motion.forward();
	}

	@Override
	protected void run(boolean toggle) {
		if (toggle) {
			// Le robot a atteint sa destination, inutile de bouger tant que
			// nouvel itinéraire pas calculé
			if (_magellan.isDestinationReached())
				setStoppable(true);
			else {
				// On signale d'emblée qu'on monopolise tant que faire se peut
				// la fonction
				setStoppable(false);
				// tourne le robot vers son cap, incline son axe puis commence à
				// avancer
				init();
			}
		} else {
			Motion.stop();
		}
	}

	@Override
	protected void tick(long time) {
		// Utile de bouger seulement si destination atteinte. Ne devrait pas
		// avoir à rester longtemps à l'arrêt ceci-dit, attente propagation
		// d'itinéraire et du bon comportement associé.
		if (!_magellan.isDestinationReached()) {
			// On est sur du blanc : sorti de la route
			if (_seeColor.getSensing() == 1) {
				if (_hasCrossedLine) {
					Diary.logln("Robot sort de la route");
					_hasCrossedLine = false;
					// change de direction seulement si on a franchi la ligne
					turn();
				}
			}
			// Tout ce qui n'est pas blanc fait partie du parcours
			else {
				if (!_hasCrossedLine) {
					Diary.logln("Robot revient sur la route");
					_hasCrossedLine = true;
				}
			}
		}
	}
}
